#include <leg_group.h>

static uint16_t deg_to_band_width(float degree);

void LG_init(LegGroup *LegG_p, UART_HandleTypeDef *huart) {
    SE_init(huart);  // 初始化舵机控制器
}

void LG_spin_to_deg(LegGroup *LG_p, float yaw, float pitch_1, float pitch_2) {
    // 设置舵机组的目标位置
    LG_p->LG_servos[0].Position = deg_to_band_width(yaw + LG_p->deg_offset[0]);
    LG_p->LG_servos[1].Position =
        deg_to_band_width(pitch_1 + LG_p->deg_offset[1]);
    LG_p->LG_servos[2].Position =
        deg_to_band_width(pitch_2 + LG_p->deg_offset[2]);

    // 设置舵机组的移动时间(默认300ms)
    uint16_t time = 3000;

    // 移动舵机组
    SE_move_servos_by_array(LG_p->LG_servos, 3, time);
}

static uint16_t deg_to_band_width(float degree) {
    uint16_t max_band_wigth = 2500;
    uint16_t min_band_wigth = 500;
    uint16_t max_deg = 270;
    uint16_t min_deg = 0;

    // 计算舵机的目标位置
    uint16_t band_width = (degree - min_deg) / (max_deg - min_deg) *
                              (max_band_wigth - min_band_wigth) +
                          min_band_wigth;
    return band_width;
}